#include "OBBboxVisitor.h"
#include <osg/MatrixTransform>
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/LineWidth>
#include <OBBbox.h>
#include <iostream>

OBBboxVisitor::OBBboxVisitor()
{
    setTraversalMode(osg::NodeVisitor::TraversalMode::TRAVERSE_ALL_CHILDREN);
    setTraversalMask(0xffffffff);
    setNodeMaskOverride(0xffffffff);//travel all node


    m_vertexs = new osg::Vec3Array();
    m_boxMT = new osg::MatrixTransform;
    m_boxGeode = new osg::Geode;
    m_boxMT->addChild(m_boxGeode);

    m_startNode = NULL;
}

OBBboxVisitor::~OBBboxVisitor()
{

}

void OBBboxVisitor::apply(osg::Node &node)
{
    if(m_startNode.get() == NULL )
    {
        m_startNode = &node;
    }
    traverse(node);

}

void OBBboxVisitor::apply(osg::MatrixTransform &node)
{
    m_matrixVector.push_back( node.getMatrix() );
    traverse(node);
    m_matrixVector.pop_back();
}

void OBBboxVisitor::apply(osg::Geode &node)
{
    updateCurrentMatrix();

    osg::Geode* geodePtr = &node;

    for(int j=0; j<geodePtr->getNumDrawables(); j++)
    {
        osg::Geometry* geom = geodePtr->getDrawable( j )->asGeometry();
        if( geom != NULL )
        {
            osg::Vec3Array* v = dynamic_cast< osg::Vec3Array* >( geom->getVertexArray() );
            for(int vIndex = 0 ; vIndex < v->size(); vIndex++)
            {
                // Transform the vertices by the specified matrix.
                osg::Vec3 newV = v->at(vIndex) * m_currentMatrix;
                m_vertexs->push_back(newV);
            }
        }
    }

    traverse(node);
}

osg::ref_ptr<osg::Node> OBBboxVisitor::getBoxMT()
{
    updateBoxMT();
    return m_boxMT;
}

void OBBboxVisitor::updateCurrentMatrix()
{
    m_currentMatrix.makeIdentity();
    for(int i = 0; i<m_matrixVector.size(); i++)
    {
        m_currentMatrix *= m_matrixVector[i];
    }
}

void OBBboxVisitor::updateBoxMT()
{
    //update geode
    m_boxGeode->removeDrawables(0, m_boxGeode->getNumDrawables());

    updateObbResult();

    osg::ref_ptr<osg::Vec3Array> m_pointVector = m_obbResult;
    osg::ref_ptr<osg::Geometry> m_geom;
    osg::ref_ptr<osg::LineWidth> m_lineWidth;
    osg::ref_ptr<osg::Vec4Array> m_colorArray;

    //add coordinate
    m_geom = new osg::Geometry;
    m_colorArray = new osg::Vec4Array;
    osg::ref_ptr<osg::Vec3Array> normalArray = new osg::Vec3Array;

    m_geom->setVertexArray( m_pointVector.get() );
    m_geom->setColorArray( m_colorArray.get() );
    m_geom->setColorBinding( osg::Geometry::BIND_OVERALL );
    m_geom->setNormalArray( normalArray.get() );
    m_geom->setNormalBinding( osg::Geometry::BIND_OVERALL );

    normalArray->push_back( osg::Vec3( 0.f,  0.f, 1.f ) );

    //set data
    m_colorArray->push_back(osg::Vec4(1,0,0,1));
    m_geom->addPrimitiveSet(new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );

    //line-size
    m_lineWidth = new osg::LineWidth;
    m_lineWidth ->setWidth(2);
    m_geom->getOrCreateStateSet()->setAttributeAndModes(m_lineWidth.get (),osg::StateAttribute::ON);

    m_boxGeode->addDrawable(m_geom);

    osg::StateSet* stateset = m_boxGeode->getOrCreateStateSet();
    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);//

    //update mt

}

void OBBboxVisitor::updateObbResult()
{
    m_obbResult = OBBBox::GetOBBCorner(m_vertexs);
    std::cout<<"OBBboxVisitor::updateObbResult size="<<m_vertexs->size()<<std::endl;

    osg::ref_ptr<osg::Vec3Array> obbVecArray = m_obbResult;
    std::cout<<"OBBboxVisitor::updateObbResult size="<<obbVecArray->size()<<std::endl;
    //std::cout<<obbVecArray->at(0).x()<<","<<obbVecArray->at(0).y()<<","<<obbVecArray->at(0).z()<<std::endl;
    //std::cout<<obbVecArray->at(1).x()<<","<<obbVecArray->at(1).y()<<","<<obbVecArray->at(1).z()<<std::endl;
    //std::cout<<obbVecArray->at(2).x()<<","<<obbVecArray->at(2).y()<<","<<obbVecArray->at(2).z()<<std::endl;
    //std::cout<<obbVecArray->at(3).x()<<","<<obbVecArray->at(3).y()<<","<<obbVecArray->at(3).z()<<std::endl;
    //std::cout<<obbVecArray->at(4).x()<<","<<obbVecArray->at(4).y()<<","<<obbVecArray->at(4).z()<<std::endl;
    //std::cout<<obbVecArray->at(5).x()<<","<<obbVecArray->at(5).y()<<","<<obbVecArray->at(5).z()<<std::endl;
    //std::cout<<obbVecArray->at(6).x()<<","<<obbVecArray->at(6).y()<<","<<obbVecArray->at(6).z()<<std::endl;
    //std::cout<<obbVecArray->at(6).x()<<","<<obbVecArray->at(6).y()<<","<<obbVecArray->at(6).z()<<std::endl;

}
